#include "SharedData.h"
#include "AxisManager.h"

constexpr int us    = 10;
constexpr int ms    = us * 1000;
constexpr int s     = ms * 1000;

SharedData *shared_data{};                      // 共享内存数据
DatasetData* _pProcessData;                     // DatasetData指针
KSHandle slave_handles[Total_Axis_Num];         // 从站句柄
KSHandle Dataset_event_handle;                  // Dataset事件句柄

KSError __stdcall timerCallBack(void* /*pArgs*/, void* /*pContext*/);
KSError __stdcall dataSetCallBack(void* /*pArgs*/, void* /*pContext*/);
KSError __stdcall stdLibraryCallBack(void* /*pArgs*/, void* /*pContext*/);
extern "C" KSError __declspec(dllexport) __stdcall InitFuction(void *pArgs, void * /*pContext*/) {
    // 1.获取共享内存数据
    shared_data = reinterpret_cast<SharedData *>(pArgs);
    // 为了同步不同实时组件，需要一个同步对象，这里使用task层的Quick mutex
    KSError ksError = KS_createQuickMutex(
        &shared_data->hMutex,                   // mutex句柄指针
        KS_RTX_LEVEL,                           // 互斥锁等级，这里是实时层
        KSF_KERNEL_EXEC);                       // 标志：内核层执行
    if (ksError != KS_OK) {
        return ksError;
    }
    std::cout << "KS_createQuickMutex" << std::endl;
    // 2. 查询主站状态
    // 也可以通过注册一个回调函数的形式来替代这种不断查询的方案，
    // 可以参见KS_installEcatHandler()的用法
    KSEcatMasterState masterState;
    masterState.structSize = sizeof(KSEcatMasterState);   // Don't forget to init structSize!

    for (int i = 0; i < 50; ++i) {
      ksError = KS_queryEcatMasterState(
        shared_data->master_handle,                         // Master handle
        &masterState,                           // Address of KSEcatMasterState structure
        KSF_NO_FLAGS);                          // Flags, here none
      if (ksError != KS_OK)
        return ksError;

      if (masterState.connected)
        break;
      KS_microDelay(100 * ms);
    }
    if (masterState.connected == 0)
      return KSERROR_CATEGORY_ETHERCAT;
    std::cout << "KS_queryEcatMasterState" << std::endl;
    // 3.Dataset操作
    // 为了PDO数据操作，必须创建一个DataSet对象并且关联一个同步对象
    // _pProcessData指针会指向DatasetData
    ksError = KS_createEcatDataSet(
        shared_data->master_handle,
        &shared_data->dataset_handle,
        nullptr,
        (void**)&_pProcessData,
        KSF_NO_FLAGS);
    if (ksError != KS_OK) {
        return ksError;
    }
    std::cout << "KS_createEcatDataSet" << std::endl;
    for (int i = 0; i < Total_Axis_Num; i++) {
        // 创建从站
        ksError = KS_createEcatSlave(
            shared_data->master_handle,
            &slave_handles[i],
            0,
            i,
            0,
            0,
            0,
            KSF_NO_FLAGS);
        if (ksError != KS_OK) {
            return ksError;
        }
        std::cout << "KS_createEcatSlave:" << i << std::endl;
        // PDO重映射
        ksError = KS_setEcatPdoMapping(slave_handles[i], 0x1601, -1, 0, 0, 0);
        if (ksError != KS_OK) {
            return ksError;
        }
        ksError = KS_setEcatPdoMapping(slave_handles[i], 0x1601, 0x6040, 0, 16, 0);
        if (ksError != KS_OK) {
            return ksError;
        }
        ksError = KS_setEcatPdoMapping(slave_handles[i], 0x1601, 0x607A, 0, 32, 0);
        if (ksError != KS_OK) {
            return ksError;
        }
        ksError = KS_setEcatPdoMapping(slave_handles[i], 0x1601, 0x60FF, 0, 32, 0);
        if (ksError != KS_OK) {
            return ksError;
        }
        ksError = KS_setEcatPdoMapping(slave_handles[i], 0x1601, 0x6071, 0, 16, 0);
        if (ksError != KS_OK) {
            return ksError;
        }
        ksError = KS_setEcatPdoMapping(slave_handles[i], 0x1601, 0x6060, 0, 8, 0);
        if (ksError != KS_OK) {
            return ksError;
        }
        ksError = KS_setEcatPdoAssign(slave_handles[i], KS_ECAT_SYNC_OUTPUT, -1, 0);
        if (ksError != KS_OK) {
            return ksError;
        }
        ksError = KS_setEcatPdoAssign(slave_handles[i], KS_ECAT_SYNC_OUTPUT, 0x1601, 0);
        if (ksError != KS_OK) {
            return ksError;
        }
        //输出通道映射
        ksError = KS_setEcatPdoMapping(slave_handles[i], 0x1A01, -1, 0, 0, 0);
        if (ksError != KS_OK) {
            return ksError;
        }
        ksError = KS_setEcatPdoMapping(slave_handles[i], 0x1A01, 0x6041, 0, 16, 0);
        if (ksError != KS_OK) {
            return ksError;
        }
        ksError = KS_setEcatPdoMapping(slave_handles[i], 0x1A01, 0x6064, 0, 32, 0);
        if (ksError != KS_OK) {
            return ksError;
        }
        ksError = KS_setEcatPdoMapping(slave_handles[i], 0x1A01, 0x606C, 0, 32, 0);
        if (ksError != KS_OK) {
            return ksError;
        }
        ksError = KS_setEcatPdoMapping(slave_handles[i], 0x1A01, 0x6077, 0, 16, 0);
        if (ksError != KS_OK) {
            return ksError;
        }
        ksError = KS_setEcatPdoMapping(slave_handles[i], 0x1A01, 0x6061, 0, 8, 0);
        if (ksError != KS_OK) {
            return ksError;
        }
        ksError = KS_setEcatPdoAssign(slave_handles[i], KS_ECAT_SYNC_INPUT, -1, 0);
        if (ksError != KS_OK) {
            return ksError;
        }
        ksError = KS_setEcatPdoAssign(slave_handles[i], KS_ECAT_SYNC_INPUT, 0x1A01, 0);
        if (ksError != KS_OK) {
            return ksError;
        }
        // 关联DataSet
        // 我们可以通过KS_assignEcatDataSet()来关联对象
        // 为了简化同步对象的绑定可以通过指定KS_ECAT_SYNC_INPUT,
        // KS_ECAT_SYNC_OUTPUT and KS_ECAT_SYNC_ALL来区分
        ksError = KS_assignEcatDataSet(
            shared_data->dataset_handle,                    // Dataset句柄
            slave_handles[i],                               // 从站句柄
            KS_ECAT_SYNC_ALL,								// 同步对象类型
            0,												// Dataset中的特殊位置
            KSF_NO_FLAGS);									// 无标志
        if (ksError != KS_OK) {
            return ksError;
        }
        std::cout << "KS_assignEcatDataSet: " << i << std::endl;
    }

    // 4.注册Dataset回调函数
    ksError = KS_createEvent(
        &Dataset_event_handle,
        NULL,
        KSF_KERNEL_EXEC);
    if (ksError != KS_OK) {
        return ksError;
    }
    std::cout << "KS_createEvent" << std::endl;
    ksError = KS_createCallBack(
        &shared_data->dataset_callback_handle,              // Dataset句柄
        dataSetCallBack,                                    // 回调函数
        nullptr,                                            // 回调函数参数
        KSF_DIRECT_EXEC,                                    // 直接执行
        0);                                                 // 优先级(内核无效)
    if (ksError != KS_OK) {
        return ksError;
    }
    ksError = KS_createTask(
        &shared_data->dataset_task_handle,                  // 任务句柄
        shared_data->dataset_callback_handle,               // 回调句柄
        131,                                                // 优先级
        KSF_DONT_START);                                    // 不启动
    if (ksError != KS_OK) {
        return ksError;
    }

    // 将创建的task以DataSet的句柄形式安装
	  ksError = KS_installEcatHandler(
		  shared_data->dataset_handle,                         // Dataset句柄
		  KS_DATASET_SIGNAL,									 // 绑定事件类型
		  shared_data->dataset_task_handle,					 // DatasetTask句柄
		  KSF_NO_FLAGS);										 // 无标志
	  if (ksError != KS_OK) {
		  return ksError;
	  }

    // 5.EtherCAT下发定时器
    ksError = KS_createCallBack(
        &shared_data->timer_callback_handle,                 // 定制器回调句柄
        timerCallBack,                                       // 回调函数
        nullptr,                                             // 回调参数
        KSF_DIRECT_EXEC,                                     // 不启动
        0);                                                  // 优先级(内核无效)
    if (ksError != KS_OK) {
        return ksError;
    }

    // 创建定时器任务
    ksError = KS_createTask(
        &shared_data->timer_task_handle,                     // 定时器task句柄地址
        shared_data->timer_callback_handle,                  // 定时器回调句柄
        130,                                                 // 优先级
        KSF_DONT_START);                                     // 标志：不立即执行
    if (ksError != KS_OK) {
        return ksError;
    }
    // 创建定时器为1ms并指定到定时task中
    ksError = KS_createTimer(
        &shared_data->timer_handle,							 // 定时器句柄指针
        1 * ms,												 // 定时周期，基础单位100ns
        shared_data->timer_task_handle,						 // 定时任务句柄
        KSF_REALTIME_EXEC | KSF_DONT_START);                 // 标志：内核层执行，不立即执行
    if (ksError != KS_OK) {
        return ksError;
    }
    // 6.DC操作
    int64ref dcStartTime = 0;
    ksError = KS_activateEcatDcMode(
        shared_data->dataset_handle,                          // Dataset句柄
        dcStartTime,                                          // 开始时间，0是未来最近的时间
        1000000,                                              // 周期时间1ms,基础单位是1ns
        0,                                                    // 转换时间,基础单位是1ns
        shared_data->timer_handle,                            // 定时器句柄
        KSF_NO_FLAGS);                                        // 无标志
    if (ksError != KS_OK && KSERROR_CODE(ksError) != KSERROR_FEATURE_NOT_LICENSED) {
        return ksError;
    }

    // 7.切换总线状态，将总线状态切换至safe OP以便可以读取PDO数据
    ksError = KS_changeEcatState(
        shared_data->dataset_handle,                          // Dataset句柄
        KS_ECAT_STATE_SAFEOP,                                 // safe OP状态
        KSF_NO_FLAGS);                                        // 无标志
    if (ksError != KS_OK) {
        return ksError;
    }
    for (int i = 0; i < Total_Axis_Num; i++) {
      _pProcessData->pdos[i].rx_pdos.control_mode = 0x08;
    }
    // 8.开启定时器
    ksError = KS_startTimer(
        shared_data->timer_handle,                            // 定时器句柄
        KSF_NO_FLAGS,                                         // 无符号
        1 * ms);                                              // 定时器周期，基础单位100ns
    if (ksError != KS_OK) {
        return ksError;
    }

    // 9.所有的准备工作已经完成，将总线状态切换到OP
    ksError = KS_changeEcatState(
        shared_data->dataset_handle,                          // DataSet句柄
        KS_ECAT_STATE_OP,                                     // Op状态
        KSF_NO_FLAGS);                                        // 无标志
    if (ksError != KS_OK) {
        return ksError;
    }

    // 10.标准库使用
    ksError = KS_createCallBack(
        &shared_data->task_callback_handle,                   // 任务回调句柄
        stdLibraryCallBack,                                         // 回调函数
        nullptr,                                              // 回调函数参数
        KSF_DIRECT_EXEC,                                      // 不启动
        0);
    if (ksError != KS_OK) {
        return ksError;
    }
    ksError = KS_setTaskStackSize(
        0x100000,
        KSF_NO_FLAGS);
    if (ksError != KS_OK) {
        return ksError;
    }
    ksError = KS_createTask(
        &shared_data->task_handle,                            // 任务句柄
        shared_data->task_callback_handle,                    // 任务回调句柄
        172,                                                  // 优先级
        KSF_CUSTOM_STACK_SIZE);
    if (ksError != KS_OK) {
        return ksError;
    }

    return KS_OK;
}

extern "C" KSError __declspec(dllexport) __stdcall ExitFuction(void * /*pArgs*/, void * /*pContext*/) {
    if (!shared_data)                                         // Shared memory not mapped!
	    return KSERROR_FUNCTION_NOT_AVAILABLE;                // _initFunction not called?
    KSError ksError;
    //------------------------------------------------------------------------------------------------------------
    // 关闭EtherCAT
    //------------------------------------------------------------------------------------------------------------
    ksError = KS_changeEcatState(
        shared_data->dataset_handle,                          // DataSet句柄
        KS_ECAT_STATE_SAFEOP,                                 // safe OP状态
        KSF_NO_FLAGS);                                        // 无标志
    if (ksError != KS_OK) {
        return ksError;
    }
    //------------------------------------------------------------------------------------------------------------
    // 关闭定时器
    //------------------------------------------------------------------------------------------------------------
    ksError = KS_stopTimer(
        shared_data->timer_handle);                           // 定时器句柄
    if (ksError != KS_OK) {
        return ksError;
    }
    //------------------------------------------------------------------------------------------------------------
    // 切换INIT状态
    //------------------------------------------------------------------------------------------------------------
    ksError = KS_changeEcatState(
        shared_data->dataset_handle,                          // DataSet句柄
        KS_ECAT_STATE_INIT,                                   // INIT状体
        KSF_NO_FLAGS);                                        // 无标志
    if (ksError != KS_OK) {
        return ksError;
    }
    //------------------------------------------------------------------------------------------------------------
    // 移除定时器
    //------------------------------------------------------------------------------------------------------------

    ksError = KS_removeTimer(
        shared_data->timer_handle);                           // 定时器句柄
    if (ksError != KS_OK) {
        return ksError;
    }
    //------------------------------------------------------------------------------------------------------------
    // 移除定时器任务
    //------------------------------------------------------------------------------------------------------------

    ksError = KS_removeTask(
        shared_data->timer_task_handle);                      // 定时器任务句柄
    if (ksError != KS_OK) {
        return ksError;
    }
    //------------------------------------------------------------------------------------------------------------
    // 移除定时器回调
    //------------------------------------------------------------------------------------------------------------
    ksError = KS_removeCallBack(
        shared_data->timer_callback_handle);                  // 定时器回调函数
    if (ksError != KS_OK) {
        return ksError;
    }
    // 向任务发出终止信号。
    shared_data->abort = 1;

    if (shared_data->task_handle) {
        ksError = KS_removeTask(shared_data->task_handle);
        if (ksError != KS_OK) {
            return ksError;
        }
    }
    if (shared_data->task_callback_handle) {
        ksError = KS_removeCallBack(shared_data->task_callback_handle);
        if (ksError != KS_OK) {
            return ksError;
        }
    }
    //------------------------------------------------------------------------------------------------------------
    // 卸载Dataset句柄
    //------------------------------------------------------------------------------------------------------------
    ksError = KS_installEcatHandler(
        shared_data->dataset_handle,                         // DataSet句柄
        KS_DATASET_SIGNAL,                                   // 绑定事件类型
        KS_INVALID_HANDLE,                                   // 清空句柄
        KSF_NO_FLAGS);                                       // 无标志
    if (ksError != KS_OK) {
        return ksError;
    }
    //------------------------------------------------------------------------------------------------------------
    // 移除事件
    //------------------------------------------------------------------------------------------------------------
    ksError = KS_closeEvent(Dataset_event_handle);
    if (ksError != KS_OK) {
        return ksError;
    }
    //------------------------------------------------------------------------------------------------------------
    // 移除Dataset任务
    //------------------------------------------------------------------------------------------------------------

    ksError = KS_removeTask(
        shared_data->dataset_task_handle);                   // Task句柄
    if (ksError != KS_OK) {
        return ksError;
    }
    //------------------------------------------------------------------------------------------------------------
    // 移除Dataset回调
    //------------------------------------------------------------------------------------------------------------
    ksError = KS_removeCallBack(
        shared_data->dataset_callback_handle);               // Dataset回调句柄
    if (ksError != KS_OK) {
        return ksError;
    }
    //------------------------------------------------------------------------------------------------------------
    // 删除DataSet.
    //------------------------------------------------------------------------------------------------------------
    ksError = KS_deleteEcatDataSet(
        shared_data->dataset_handle);                         // DataSet句柄
    if (ksError != KS_OK) {
        return ksError;
    }
    //------------------------------------------------------------------------------------------------------------
    // 从站切换INIT状态
    //------------------------------------------------------------------------------------------------------------
    for (int i = 0; i < Total_Axis_Num; ++i) {
        ksError = KS_changeEcatState(
            slave_handles[i],                       // 从站句柄
            KS_ECAT_STATE_INIT,                     // INIT状态
            KSF_NO_FLAGS);                          // 无标志
        if (ksError != KS_OK)
            return ksError;

    }
    //------------------------------------------------------------------------------------------------------------
    // 主站需要在完成后切换到INIT状态
    //------------------------------------------------------------------------------------------------------------

    ksError = KS_changeEcatState(
        shared_data->master_handle,                 // 主站句柄
        KS_ECAT_STATE_INIT,                         // INIT状态
        KSF_NO_FLAGS);                              // 无标志
    if (ksError != KS_OK) {
        return ksError;
    }
    //------------------------------------------------------------------------------------------------------------
    // 删除从站
    //------------------------------------------------------------------------------------------------------------
    for (int i = 0; i < Total_Axis_Num; ++i) {
        ksError = KS_deleteEcatSlave(
            slave_handles[i]);                      // 从站句柄
        if (ksError != KS_OK) {
            return ksError;
        }
    }
    //------------------------------------------------------------------------------------------------------------
    // 关闭主站
    //------------------------------------------------------------------------------------------------------------
    ksError = KS_closeEcatMaster(
        shared_data->master_handle);               // 主站句柄
    if (ksError != KS_OK) {
        return ksError;
    }
    //------------------------------------------------------------------------------------------------------------
    // 关闭网卡
    //------------------------------------------------------------------------------------------------------------
    ksError = KS_closeNetwork(
        shared_data->adapter_handle,              // 网卡句柄
        KSF_NO_FLAGS);                            // 无标志
    if (ksError != KS_OK) {
        return ksError;
    }
    //------------------------------------------------------------------------------------------------------------
    // 移除互斥
    //------------------------------------------------------------------------------------------------------------
    ksError = KS_removeQuickMutex(
        shared_data->hMutex);                     // 互斥句柄
    if (ksError != KS_OK) {
        return ksError;
    }
    return KS_OK;
}


KSError __stdcall timerCallBack(void* /*pArgs*/, void* /*pContext*/) {
	KSError ksError;
	if (shared_data->error != KS_OK) {
		return shared_data->error;
	}

	ksError = KS_requestQuickMutex(
		shared_data->hMutex);
	if (ksError != KS_OK) {
		shared_data->error = ksError;
		return ksError;
	}

	ksError = KS_postEcatDataSet(
		shared_data->dataset_handle,
		KSF_NO_FLAGS);
	if (ksError != KS_OK) {
		shared_data->error = ksError;
	}

	ksError = KS_releaseQuickMutex(
		shared_data->hMutex);
	if (ksError != KS_OK) {
		shared_data->error = ksError;
		return ksError;
	}
	return shared_data->error;
}

KSError __stdcall dataSetCallBack(void* /*pArgs*/, void* /*pContext*/) {
    KSError ksError;
	static bool running = false;

	if (shared_data->error != KS_OK)
		return shared_data->error;

	ksError = KS_requestQuickMutex(
		shared_data->hMutex);                    // Mutex handle
	if (ksError != KS_OK) {
		shared_data->error = ksError;
		return ksError;
	}
	ksError = KS_readEcatDataSet(
		shared_data->dataset_handle,              // DataSet handle
		KSF_NO_FLAGS);                            // Flags (unused)
	if (ksError != KS_OK) {
		if (!running && KSERROR_CODE(ksError) == KSERROR_NO_RESPONSE) {
			// This error indicates that a slave did not answer to KS_postEcatDataSet().
			// Some slaves need more time to fully arrive in SAFEOP and answer to the DataSet.
			// So we are simply ignoring this error here.
			// A real world application should handle this error differently during startup and running phase.
		}
		else {
			shared_data->error = ksError;
			// KS_readEcatDataSet() successfully returned data, the slave has arrived in SAFEOP or higher from now on
			// we are handling each error seriously.
		}
	}
	else {
		running = true;
        for (int i = 0; i < Total_Axis_Num; ++i) {
		    shared_data->actual_position[i] = _pProcessData->pdos[i].tx_pdos.current_pos;
            shared_data->control_words[i] = _pProcessData->pdos[i].rx_pdos.control_word;
            shared_data->statues[i] = _pProcessData->pdos[i].tx_pdos.status_word;
        }
        KS_setEvent(Dataset_event_handle);
    }

    ksError = KS_releaseQuickMutex(
        shared_data->hMutex);                           // Mutex handle
    if (ksError != KS_OK) {
        shared_data->error = ksError;
        return ksError;
    }

	return shared_data->error;
}

KSError __stdcall stdLibraryCallBack(void* /*pArgs*/, void* /*pContext*/) {
    AxisManager axis_manager;
    for (int i = 0; i < Total_Axis_Num; i++) {
        axis_manager.AddDriver(&_pProcessData->pdos[i]);
        std::cout << "AddDriver rx_pdo:" << &(_pProcessData->pdos[i].rx_pdos) << std::endl;
    }
    while (true) {
        KS_waitForEvent(Dataset_event_handle, KSF_NO_FLAGS, 3 * ms);
        if (shared_data->abort) {
            break;
        }
        KSError ksError = KS_requestQuickMutex(
        shared_data->hMutex);                    // Mutex handle
        if (ksError != KS_OK) {
            shared_data->error = ksError;
            return ksError;
        }
        axis_manager.TimeTick();
        switch (shared_data->command_type) {
        case 1:
            std::cout << "-----------kernel : Enable Command Received----------" << std::endl;
            for (int i = 0; i < Total_Axis_Num; i++) {
              axis_manager.PowerOn(i);
            }
            shared_data->command_type = 0;
        break;
        case 2:
            std::cout << "-----------kernel : Disable Command Received----------" << std::endl;
            for (int i = 0; i < Total_Axis_Num; i++) {
                axis_manager.PowerOff(i);
            }
            shared_data->command_type = 0;
        break;
        case 3:
            std::cout << "-----------kernel : Home Command Received----------" << std::endl;
            axis_manager.Home(0);
            shared_data->command_type = 0;
        break;
        case 4:
            std::cout << "-----------kernel : Home Cancel Command Received----------" << std::endl;
            axis_manager.CancelHome(0);
            shared_data->command_type = 0;
        break;
        }
        ksError = KS_releaseQuickMutex(
		shared_data->hMutex);                           // Mutex handle
	    if (ksError != KS_OK) {
            shared_data->error = ksError;
            return ksError;
        }
    }
    return KS_OK;
}

#pragma pack(push, 8)
#include <windows.h>
#pragma pack(pop)

BOOL WINAPI DllMain(HINSTANCE hInstDll, DWORD reason, LPVOID pReserved)
{
    return TRUE;
}